﻿#include "HandMeasureFunction.h"

HandMeasureFunction::HandMeasureFunction(HandModelRenderer* renderer,OpenCLManager* ocl)
{
	this->renderer = renderer;
	this->oclManager = ocl;
	oclManager->setupOCL();
	oclManager->setupMatchingKernel_Pixel(renderer->renBuffer,
		renderer->obsDepthBuffer,renderer->obsSkinBuffer);
}

HandMeasureFunction::~HandMeasureFunction(void)
{
}

double HandMeasureFunction::getE_Vector(double D_Results, double * hypos)
{
	double phi1,phi2,phi3;
	// ngon tro & ngon giua
	phi1 = hypos[OFFSET_FINGER1] - hypos[OFFSET_FINGER2]; 
	if (phi1>0) phi1 = 0;
	// ngon giua va ngon ap ut
	phi2 = hypos[OFFSET_FINGER2] - hypos[OFFSET_FINGER3];
	if (phi2>0) phi2 = 0;
	// ngon ap ut va ngon ut
	phi3 = hypos[OFFSET_FINGER3] - hypos[OFFSET_LITTLE];
	if (phi3>0) phi3 = 0;
	// Sum in Radian for a penalty 
	double sum_phi = (-phi1 - phi2 - phi3) * 3.14/180;

	return D_Results + (LAMDA_K * sum_phi);
}

void HandMeasureFunction::generateObserve(HypothesisX hypo)
{
	int swarmParticles = NUM_TOTAL_PARTICLES;
	std::vector<HypothesisX> hypoArr(swarmParticles);
	hypoArr.assign(swarmParticles,hypo);

	renderer->renderMultiModelGL(hypoArr);
	renderer->putDepthToBuf(renderer->obsDepthBuffer);
	renderer->putDepthToBuf(renderer->obsSkinBuffer);
	oclManager->acquireObservePixel();
}

void HandMeasureFunction::psoFunction(std::vector<double> &measureResult)
{
	size_t maxParticles = measureResult.size();
	measureResult.assign(maxParticles,4);
	renderer->renderMultiModelGL(hypoArrays);
	renderer->putDepthToBuf(renderer->renBuffer);
	oclManager->runCLKernel_GetDforSwarm_Pixels(measureResult);

	for (int i=0;i<maxParticles;i++){
		measureResult[i]=getE_Vector(measureResult[i],hypoArrays[i].dofData);
	}
}

void HandMeasureFunction::psoVerifying(std::vector<double> &measureResult)
{
}